/**
 * @file Path.cpp
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief 路径类
 */

#include "Path.h"
#include "AStar.h"
#include "Bench.h"
#include <cstring>

int Path::robotRegion[ROBOT_NUM_MAX] = {0};
int Path::benchRegion[BENCH_NUM_MAX] = {0};
int Path::benchRegionFull[BENCH_NUM_MAX] = {0};
std::list<Cmd> Path::cmdSeqEmpty[BENCH_NUM_MAX][BENCH_NUM_MAX];
std::list<Cmd> Path::cmdSeqFull[BENCH_NUM_MAX][BENCH_NUM_MAX];
bool Path::hadCmdSeqEmpty[BENCH_NUM_MAX][BENCH_NUM_MAX] = {0};
bool Path::hadCmdSeqFull[BENCH_NUM_MAX][BENCH_NUM_MAX] = {0};

/**
 * @brief 初始化空载地图下机器人与工作台所在区域
 */
void Path::initEmptyRegion() {
    int regionID = 1; // 区域号
    for(int id = 0; id < ROBOT_NUM_MAX; ++id) {
        if(Path::robotRegion[id]) { // 已经划分了区域
            continue;
        } else {
            Path::robotRegion[id] = regionID++;
        }
        bool visited[MAP_X_MAX_EXPAND][MAP_Y_MAX_EXPAND]; // 节点是否已访问
        std::memset(visited, false, sizeof visited);
        Map::robotLocate *robotLocate = Map::getRobotLocate(id);
        std::list<MapNode *> openSet; // 开集
        openSet.push_back(new MapNode(robotLocate->x, robotLocate->y, Direction::NONE));
        visited[robotLocate->x][robotLocate->y] = true;
        for(MapNode *current = openSet.front(); !openSet.empty(); current = openSet.front()) {
            int isRobot = Map::isRobot(current->x, current->y, id, true);
            if(isRobot != -1) {
                Path::robotRegion[isRobot] = Path::robotRegion[id];
            }
            int isBench = Map::isBench(current->x, current->y);
            if(isBench != -1) {
                Path::benchRegion[isBench] = Path::robotRegion[id];
            }
            for(int dir = 1; dir <= 8; ++dir) { // 0方向为原地不动
                MapNode *child = new MapNode(current->x, current->y, dir);
                if(Map::isInMap(child->x, child->y) && !Map::isObstacle(child->x, child->y, true) && !visited[child->x][child->y]) {
                    visited[child->x][child->y] = true;
                    openSet.push_back(child);
                } else {
                    delete child;
                    child = nullptr;
                }
            }
            openSet.pop_front();
            delete current;
            current = nullptr;
        }
    }
}

/**
 * @brief 初始化满载地图下工作台所在区域
 */
void Path::initFullRegion() {
    int regionID = 1; // 区域号
    for(int id = 0; id < Bench::getTotalNumber(); ++id) {
        if(Path::benchRegionFull[id]) { // 已经划分了区域
            continue;
        } else {
            Path::benchRegionFull[id] = regionID++;
        }
        bool visited[MAP_X_MAX_EXPAND][MAP_Y_MAX_EXPAND]; // 节点是否已访问
        std::memset(visited, false, sizeof visited);
        Map::benchLocate *benchLocate = Map::getBenchLocate(id);
        std::list<MapNode *> openSet; // 开集
        openSet.push_back(new MapNode(benchLocate->x, benchLocate->y, Direction::NONE));
        visited[benchLocate->x][benchLocate->y] = true;
        for(MapNode *current = openSet.front(); !openSet.empty(); current = openSet.front()) {
            int isBench = Map::isBench(current->x, current->y);
            if(isBench != -1) {
                Path::benchRegionFull[isBench] = Path::benchRegionFull[id];
            }
            for(int dir = 1; dir <= 8; ++dir) { // 0方向为原地不动
                MapNode *child = new MapNode(current->x, current->y, dir);
                if(Map::isInMap(child->x, child->y) && !Map::isObstacle(child->x, child->y, false) && !visited[child->x][child->y]) {
                    visited[child->x][child->y] = true;
                    openSet.push_back(child);
                } else {
                    delete child;
                    child = nullptr;
                }
            }
            openSet.pop_front();
            delete current;
            current = nullptr;
        }
    }
}

/**
 * @brief 初始化
 */
void Path::init() {
    Path::initEmptyRegion();
    Path::initFullRegion();
}

/**
 * @brief 判断工作台之间是否可达
 * @param start 起点工作台
 * @param end 终点工作台
 * @param isEmpty 是否使用空载地图
 * @return true 可达
 * @return false 不可达
 */
bool Path::isArrival_BenchToBench(int start, int end, bool isEmpty) {
    if(isEmpty) {
        return Path::benchRegion[start] == Path::benchRegion[end];
    } else {
        return Path::benchRegionFull[start] == Path::benchRegionFull[end];
    }
}

/**
 * @brief 判断机器人是否可达工作台
 * @param robot 机器人ID
 * @param bench 工作台ID
 * @return true 可达
 * @return false 不可达
 */
bool Path::isArrival_RobotToBench(int robot, int bench) {
    return Path::robotRegion[robot] == Path::benchRegion[bench];
}

/**
 * @brief 获得在目标两个工作台间移动的指令序列
 * @param StartBench 起点工作台ID
 * @param EndBench 终点工作台ID
 * @param isEmpty 是否使用空载地图
 * @return std::list<Cmd> 指令序列
 */
std::list<Cmd> Path::getCmdSeq_BenchToBench(int StartBench, int EndBench, bool isEmpty) {
    std::list<Cmd> ret;
    if(Path::isArrival_BenchToBench(StartBench, EndBench, isEmpty)) { // 在同一区域内才存在路径
        if(isEmpty) {
            if(Path::hadCmdSeqEmpty[StartBench][EndBench]) {
                ret = Path::cmdSeqEmpty[StartBench][EndBench];
            } else if(Path::hadCmdSeqEmpty[EndBench][StartBench]) {
                ret = Path::cmdSeqEmpty[EndBench][StartBench];
                ret.reverse();
                cmdSeqEmpty[EndBench][StartBench] = ret;
                Path::hadCmdSeqEmpty[EndBench][StartBench] = true;
            } else {
                Map::benchLocate *start = Map::getBenchLocate(StartBench);
                Map::benchLocate *end = Map::getBenchLocate(EndBench);
                ret = cmdSeqEmpty[StartBench][EndBench] = AStar::getCmdSeq(start->x, start->y, end->x, end->y, isEmpty, 1);
                Path::hadCmdSeqEmpty[StartBench][EndBench] = true;
            }
        } else {
            if(Path::hadCmdSeqFull[StartBench][EndBench]) {
                ret = Path::cmdSeqFull[StartBench][EndBench];
            } else if(Path::hadCmdSeqFull[EndBench][StartBench]) {
                ret = Path::cmdSeqFull[EndBench][StartBench];
                ret.reverse();
                cmdSeqFull[EndBench][StartBench] = ret;
                Path::hadCmdSeqFull[EndBench][StartBench] = true;
            } else {
                Map::benchLocate *start = Map::getBenchLocate(StartBench);
                Map::benchLocate *end = Map::getBenchLocate(EndBench);
                ret = cmdSeqFull[StartBench][EndBench] = AStar::getCmdSeq(start->x, start->y, end->x, end->y, isEmpty, 1);
                Path::hadCmdSeqFull[StartBench][EndBench] = true;
            }
        }
    }
    return ret;
}

/**
 * @brief 获得机器人移动到指定工作台的指令序列
 * @param robot 机器人ID
 * @param bench 工作台ID
 * @param isEmpty 是否使用空载地图
 * @return std::list<Cmd> 指令序列
 */
std::list<Cmd> Path::getCmdSeq_RobotToBench(int robot, int bench, bool isEmpty) {
    std::list<Cmd> ret;
    if(Path::isArrival_RobotToBench(robot, bench)) { // 在同一区域内才存在路径
        Map::robotLocate *start = Map::getRobotLocate(robot);
        Map::benchLocate *end = Map::getBenchLocate(bench);
        ret = AStar::getCmdSeq(start->x, start->y, end->x, end->y, isEmpty, 1);
    }
    return ret;
}

/**
 * @brief 获得机器人买卖物品的完整指令序列
 * @param robot 机器人
 * @param buy 购买工作台
 * @param sell 出售工作台
 * @return std::list<Cmd> 指令序列
 */
std::list<Cmd> Path::getCmdSeq(Robot *robot, Bench *buy, Bench *sell) {
    std::list<Cmd> ret;
    if(Path::isArrival_RobotToBench(robot->getID(), buy->getID()) && Path::isArrival_BenchToBench(buy->getID(), sell->getID(), false)) { // 在同一区域内才存在路径
        ret.splice(ret.end(), Path::getCmdSeq_RobotToBench(robot->getID(), buy->getID(), true));
        ret.push_back(Cmd::createBuy(buy));
        ret.splice(ret.end(), Path::getCmdSeq_BenchToBench(buy->getID(), sell->getID(), false));
        ret.push_back(Cmd::createSell(sell, buy->getType()));
    }
    return ret;
}

/**
 * @brief 获得目标工作台间的距离
 * @param StartBench 起点工作台ID
 * @param EndBench 终点工作台ID
 * @param isEmpty 是否使用空载地图
 * @return int 无冲突情况下的步数(-1为不可达)
 */
int Path::getDistance_BenchToBench(int StartBench, int EndBench, bool isEmpty) {
    return Path::getCmdSeq_BenchToBench(StartBench, EndBench, isEmpty).size();
}

/**
 * @brief 获得机器人到指定工作台的距离
 * @param robot 机器人ID
 * @param bench 工作台ID
 * @return int 无冲突情况下的步数(-1为不可达)
 */
int Path::getDistance_RobotToBench(int robot, int bench) {
    Map::robotLocate *robotLoc = Map::getRobotLocate(robot);
    for(int id = 0; id < Bench::getTotalNumber(); ++id) { // 如果机器人在某个工作台处，使用工作台间的距离简化计算
        Map::benchLocate *benchLoc = Map::getBenchLocate(id);
        if(benchLoc->x == robotLoc->x && benchLoc->y == robotLoc->y) {
            return Path::getDistance_BenchToBench(id, bench, robotLoc->isEmpty);
        }
    }
    return Path::getCmdSeq_RobotToBench(robot, bench, Map::getRobotLocate(robot)->isEmpty).size();
}
